#include <behaviortree_cpp_v3/bt_factory.h>
#include <behaviortree_cpp_v3/loggers/bt_zmq_publisher.h>
#include <iostream>
using namespace std;

class PrintMessage : public BT::SyncActionNode
{
private:
    int count_;
public:
    PrintMessage(const std::string& name, const BT::NodeConfiguration& config)
        : SyncActionNode(name, config)
    {
        count_ = 0;
    }

    // tick 执行函数
    BT::NodeStatus tick() override
    {
        cout << "hello BehaviorTreeCpp V3: " << count_ << endl;
        if(count_++ %2 == 0)
            return BT::NodeStatus::SUCCESS;
        else
            return BT::NodeStatus::FAILURE;
    }

    // 提供 输入/输出 端口
    static BT::PortsList providedPorts()
    {
        return {};
    }
};

int main()
{
    // 行为工厂
    BT::BehaviorTreeFactory factory;
    // 注册自定义节点
    factory.registerNodeType<PrintMessage>("PrintMessage");
    // 定义行为树 xml结构
    string xml_tree = R"(
<root BTCPP_format="3">
    <BehaviorTree ID="MainTree">
        <PrintMessage name="print_hello"/>
    </BehaviorTree>
</root>
)";
    // 从xml 加载行为树
    BT::Tree tree = factory.createTreeFromText(xml_tree);
    BT::PublisherZMQ publisher_zmq(tree);

    // 执行行为树
    for (int i = 0; i < 100000; i++)
    {
        tree.tickRoot();
        tree.sleep(std::chrono::seconds(1));
    }

    return 0;
}